#include "main.h"
#include "string.h"

#define CH_TRACE printf

#define CHSYNC1         (0x5A)        /* CHAOHE message sync code 1 */
#define CHSYNC2         (0xA5)        /* CHAOHE message sync code 2 */
#define CH_HDR_SIZE     (0x06)        /* CHAOHE protocol header size */

#define U1(p) (*((uint8_t *)(p)))
#define I1(p) (*((int8_t  *)(p)))
#define I2(p) (*((int16_t  *)(p)))

#define  CH_DEBUG
#define MAXRAWLEN       (512)

static uint16_t U2(uint8_t *p) {uint16_t u; memcpy(&u,p,2); return u;}
static uint32_t U4(uint8_t *p) {uint32_t u; memcpy(&u,p,4); return u;}
static float    R4(uint8_t *p) {float    r; memcpy(&r,p,4); return r;}
typedef struct
{
    uint32_t    id;            /* user defined ID       */
    int8_t      temperature;
    float       acc[3];           /* acceleration          */
    float       gyr[3];           /* angular velocity      */  
    float       mag[3];           /* magnetic field        */
    float       eul[3];           /* attitude: eular angle */
    float       quat[4];          /* attitude: quaternion  */
    float       pressure;         /* air pressure          */
    uint32_t    timestamp;
}ch_imu_data_t;

typedef struct
{
    int nbyte;                          /* number of bytes in message buffer */ 
    int len;                            /* message length (bytes) */
    uint8_t buf[MAXRAWLEN];             /* message raw buffer */
   
    ch_imu_data_t imu;   /* imu data list, if (HI226/HI229/CH100/CH110, use imu[0]) */
}raw_t;

static int sync_ch(uint8_t *buf, uint8_t data);

extern uint8_t rx_buf[10];
extern raw_t raw;   


int ch_serial_input(raw_t *raw, uint8_t data);
static int decode_ch(raw_t *raw);
static void crc16_update(uint16_t *currect_crc, const uint8_t *src, uint32_t len);
static int parse_data(raw_t *raw);